﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Drawing;
using Point2d.Graphics;
using Point2d.Geometry;

namespace Point2d.App
{
    public class Vehicle : LinesEntity
    {
        Lane _lane = null;
        public Status _status = new Status();
        public float _length = 1.0f;
        public float _width = 4.0f;

        List<Wheel> _wheels = new List<Wheel>();

        public Vehicle(GraphicsManager manager, Color color, Lane lane)
            : base(manager, color)
        {
            _lane = lane;
            for (int i = 0; i < 4; i ++)
            {
                _wheels.Add(new Wheel(manager, color, this));
            }
            _wheels[0].Create(Wheel.Type.eHeadLeft, -0.1f, -0.5f);
            _wheels[1].Create(Wheel.Type.eHeadRight, -0.1f, 0.5f);
            _wheels[2].Create(Wheel.Type.eBackLeft, -0.9f, -0.5f);
            _wheels[3].Create(Wheel.Type.eHeadRight, -0.9f, 0.5f);

            for (int i = 0; i < 16; i ++)
            {
                AddPoint(0, 0);
            }
        }

        public void GetPosition(ref float x, ref float y)
        {
            float moveDirX = 0.0f;
            float moveDirY = 0.0f;
            _lane.GetDirection(ref moveDirX, ref moveDirY);
            float laneLength = _lane.GetLength();

            float relativeDirX = 0.0f;
            float relativeDirY = 0.0f;
            MathUtil.RotateDir(moveDirX, moveDirY, 90, ref relativeDirX, ref relativeDirY);

            x = _lane._from._x + moveDirX * _status._pos * laneLength + relativeDirX * _status._posLat * _lane._width;
            y = _lane._from._y + moveDirY * _status._pos * laneLength + relativeDirY * _status._posLat * _lane._width;
        }

        public void GetDirection(ref float directionX, ref float directionY)
        {
            float laneDirX = 0.0f;
            float laneDirY = 0.0f;
            _lane.GetDirection(ref laneDirX, ref laneDirY);
            MathUtil.RotateDir(laneDirX, laneDirY, _status._angle, ref directionX, ref directionY);
        }

        public void GetHeadPoint(ref float x, ref float y)
        {
            GetPosition(ref x, ref y);
        }

        public void GetBackPoint(ref float x, ref float y)
        {
            GetPosition(ref x, ref y);

            float directionX = 0.0f;
            float directionY = 0.0f;
            GetDirection(ref directionX, ref directionY);

            x = x - directionX * _length;
            y = y - directionY * _length;
        }

        public void Rotate(float delta)
        {
            _status._angle += delta;
            Update();
        }

        public void RelativeMove(float delta)
        {
            _status._posLat += delta;
            Update();
        }

        public void PosMove(float delta)
        {
            _status._pos += delta;
            Update();
        }

        public void Update()
        {
            // get relative direction
            float directionX = 0.0f;
            float directionY = 0.0f;
            GetDirection(ref directionX, ref directionY);
            float relativeDirX = 0.0f;
            float relativeDirY = 0.0f;
            MathUtil.RotateDir(directionX, directionY, 90, ref relativeDirX, ref relativeDirY);

            float headX = 0, headY = 0;
            GetHeadPoint(ref headX, ref headY);
            float backX = 0, backY = 0;
            GetBackPoint(ref backX, ref backY);

            SetPoint(0, headX, headY);
            SetPoint(1, backX + relativeDirX * _width / 2.0f, backY + relativeDirY * _width / 2.0f);

            SetPoint(2, headX, headY);
            SetPoint(3, backX - relativeDirX * _width / 2.0f, backY - relativeDirY * _width / 2.0f);

            SetPoint(4, backX + relativeDirX * _width / 2.0f, backY + relativeDirY * _width / 2.0f);
            SetPoint(5, backX - relativeDirX * _width / 2.0f, backY - relativeDirY * _width / 2.0f);

            SetPoint(6, headX, headY);
            SetPoint(7, backX, backY);

            SetPoint(8, backX + relativeDirX * _width / 2.0f, backY + relativeDirY * _width / 2.0f);
            SetPoint(9, headX + relativeDirX * _width / 2.0f, headY + relativeDirY * _width / 2.0f);
            SetPoint(10, headX + relativeDirX * _width / 2.0f, headY + relativeDirY * _width / 2.0f);
            SetPoint(11, headX, headY);

            SetPoint(12, backX - relativeDirX * _width / 2.0f, backY - relativeDirY * _width / 2.0f);
            SetPoint(13, headX - relativeDirX * _width / 2.0f, headY - relativeDirY * _width / 2.0f);
            SetPoint(14, headX - relativeDirX * _width / 2.0f, headY - relativeDirY * _width / 2.0f);
            SetPoint(15, headX, headY);
        }

        public void Set(float pos, float angle, float relativePs)
        {
            _status._pos = pos;
            _status._posLat = relativePs;
            _status._angle = angle;
        }
    }
}
